//
// Created by oftenlin on 23-9-13.
//
#include "NaviFusionImpl.hpp"
#include <iostream>
void INaviFusion::Measurement(Eigen::VectorXd &z, Eigen::MatrixXd &H_, Eigen::MatrixXd &R_, Eigen::VectorXd &x_,
                              Eigen::MatrixXd &xp_) {
    Eigen::MatrixXd Ht = H_.transpose();
    Eigen::MatrixXd PHt = xp_ * Ht;

    Eigen::VectorXd y = z;
    Eigen::MatrixXd S = H_ * PHt + R_;
    Eigen::MatrixXd K = PHt * S.inverse();
    x_ = x_ + (K * y);
    long x_size = x_.size();
    Eigen::MatrixXd I =  Eigen::MatrixXd::Identity(x_size, x_size);
    xp_ = (I - K * H_) * xp_;
    xp_ = 0.5 * (xp_ + xp_.transpose());
}